/*
 * TODO: Description
 *
 * Copyright (C) 2010 Johan Waldeback
 *  
 * This file is part of the OpenFMS project (http://www.openfms-project.org)
 * 
 * OpenFMS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "FmsVehicle.h"
#include <cassert>

#include <OgreBulletDynamicsRigidBody.h>
//#include <OgreBulletDynamicsWorld.h>
//#include <OgreBulletCollisionsWorld.h>


// todo: when this filehas taken over the functions of he Tick Listener, remove that file(?)


using namespace OpenFms;

/** 
 * Vehicle class constructor
 */
FmsVehicle::FmsVehicle() :
	m_pSceneNode(NULL),
	m_pInputDevice(NULL),
	m_pEntity(NULL),
	m_simulator(),
	m_pCollisionShape(NULL),
	m_pRigidBody(NULL),
	m_pWheelShape(NULL),
	m_pFrontWheel(NULL),
	m_pFrontWheelConstraint(NULL)
{
	memset( &m_inertias, 0, sizeof m_inertias );
	m_inertias.size = sizeof m_inertias;

	memset( &m_environment, 0, sizeof(ModelSimulator_environment_t));
	m_environment.size = sizeof(ModelSimulator_environment_t);
}

/** 
 * Vehicle class destructor
 */
FmsVehicle::~FmsVehicle() 
{
	unload();
	BulletTickListener::~BulletTickListener();
}


static bool findResourceEndsWith( Ogre::String &result, const Ogre::String &pattern, const Ogre::String &group )
{
	Ogre::StringVectorPtr names = Ogre::ResourceGroupManager::getSingletonPtr()->listResourceNames( group );
	for( Ogre::StringVector::iterator iter = names->begin(); iter != names->end(); ++iter ){
		if( Ogre::StringUtil::endsWith( *iter, pattern)) {
			result = *iter;
			return true;
		}
	}
	return false;
}

size_t FmsVehicle::m_uniqueCount = 0;
Ogre::String FmsVehicle::generateUniqueName()
{
	char buffer[10];
	itoa( ++m_uniqueCount , buffer, 10 );
	return Ogre::String( "FmsVehicle_") + Ogre::String(buffer) ;
}

/**
 * Given a Ogre resource group name, find the par file in that group
 *
 * @param[out] result Name of found par file
 * @param[in] group Name of group so search
 * @return Returns true if a .par file was found, false if not.
 */
bool FmsVehicle::findParName( Ogre::String &result, const Ogre::String &group )
{
	return findResourceEndsWith( result, Ogre::String(".par"), group );
}

/**
 * Given a Ogre resource group name, find the mesh file in that group
 *
 * @param[out] result Name of found mesh file
 * @param[in] group Name of group so search
 * @return Returns true if a .mesh file was found, false if not.
 */
bool FmsVehicle::findMeshName( Ogre::String &result, const Ogre::String &group )
{
	return findResourceEndsWith( result, Ogre::String(".mesh"), group );
}

/**
 * Given a Ogre resource group name, find the material file in that group
 *
 * @param[out] result Name of found the material
 * @param[in] group Name of group so search
 * @return Returns true if a material file was found, false if not.
 */
bool FmsVehicle::findMaterialName( Ogre::String &result, const Ogre::String &group )
{
	if( findResourceEndsWith( result, Ogre::String(".material"), group ) == false ) return false;
	int length = result.length();
	// if the name is ".material", then fail.
	if( length <= 9 ) { return false; }

	// extract "xyz" from "xyz.material"
	result = result.substr( 0, length-9 );
	return true;
}

/**
 * Load a vehicle from a ogre resource group
 *
 * Creates visual and physical models, and loads a simulator.
 *
 * @return Returns true on success, false on fail.
 */
// todo error messages
bool FmsVehicle::load( const Ogre::String &group, OgreBulletDynamics::DynamicsWorld *pWorld, Ogre::SceneManager *pSceneManager  )
{
	// first unload anything that is already loaded
	unload();
	assert( m_pEntity == NULL && "There should not be an entity (yet)" );
	assert( m_pCollisionShape == NULL && "There should not be a collision shape (yet)");
	assert( m_pRigidBody == NULL && "There should be no rigid body (yet)");
	assert( m_pSceneNode == NULL && "There should be no scene node (yet)");

	// Create simulator, and load some basic data
	Ogre::String parName;
	if( findParName( parName, group ) == false ) {
		Ogre::LogManager::getSingletonPtr()->logMessage( "ERROR (FmsVehicle::load): Unable to find .par file" );
		return false;
	}
	Ogre::LogManager::getSingletonPtr()->logMessage( Ogre::String("FmsVehicle::load - Found .par : ")+parName );

	Ogre::String meshName;
	if( findMeshName( meshName, group ) == false ) {
		Ogre::LogManager::getSingletonPtr()->logMessage( "ERROR (FmsVehicle::load): Unable to find .mesh file" );
		return false;
	}
	Ogre::LogManager::getSingletonPtr()->logMessage( Ogre::String("FmsVehicle::load - Found .mesh : ")+meshName );

	Ogre::String materialName;
	if( findMaterialName( materialName, group ) == false ) {
		Ogre::LogManager::getSingletonPtr()->logMessage( "ERROR (FmsVehicle::load): Unable to find material " );
		return false;
	}
	Ogre::LogManager::getSingletonPtr()->logMessage( Ogre::String("FmsVehicle::load - Found material : ")+materialName );

	// Load simulator
	if( loadSimulator( _T("Fms7Aero.dll"), parName, group ) == false ) return false;

	// Get some data
	m_inertias.size = sizeof m_inertias; // just in case getInertias changes this.
	m_simulator.getInertias( m_inertias );

	// Create and attach visual object
	m_pSceneNode = pSceneManager->getRootSceneNode()->createChildSceneNode( generateUniqueName() );
	m_pEntity = pSceneManager->createEntity(generateUniqueName(), meshName, group );
	m_pEntity->setMaterialName( materialName, group );
	m_pSceneNode->attachObject( m_pEntity );

	// Create physical object
	Ogre::Vector3 size = (m_pEntity->getBoundingBox()).getHalfSize();
	m_pCollisionShape = new OgreBulletCollisions::BoxCollisionShape( Ogre::Vector3(size.x, size.y, .5f * size.z) );
	m_pRigidBody = new OgreBulletDynamics::RigidBody( generateUniqueName(), pWorld );
	
	const float mass = m_inertias.mass;
	const Ogre::Vector3 position(0,0,-1.f);
	const Ogre::Quaternion orientation(0,0,0,1);
	const float restitution = .1f;
	const float friction = .8f;
	m_pRigidBody->setShape( m_pSceneNode, m_pCollisionShape, restitution, friction, mass, position, orientation );
	m_pRigidBody->getBulletRigidBody()->setActivationState(DISABLE_DEACTIVATION); // !!!

	// Create wheels
	// note: rotation axis is Z, This semms to be the deafult in bullet, 
	// and can be worked around, but easier just to go with the flow.
	Ogre::SceneNode *pSceneNode;
	m_pWheelShape = new OgreBulletCollisions::CylinderCollisionShape(Ogre::Vector3(size.z, size.z, size.z), Ogre::Vector3(0,0,1) );
	m_pFrontWheel = new OgreBulletDynamics::RigidBody( generateUniqueName(), pWorld );
	m_pRearWheel = new OgreBulletDynamics::RigidBody( generateUniqueName(), pWorld );

	const Ogre::Vector3 frontPosition = Ogre::Vector3(size.x,0,0);
	const Ogre::Quaternion frontOrientation( Ogre::Radian(SIMD_PI/2), Ogre::Vector3(1,0,0));
	const Ogre::Vector3 rearPosition = Ogre::Vector3(-size.x,0,0);
	const Ogre::Quaternion rearOrientation(Ogre::Radian(SIMD_PI/2), Ogre::Vector3(1,0,0));

	// setup front wheel
	pSceneNode = pSceneManager->getRootSceneNode()->createChildSceneNode( generateUniqueName() );
	pSceneNode->attachObject( pSceneManager->createEntity( Ogre::SceneManager::PT_CUBE ) );
	pSceneNode->setScale( Ogre::Vector3(size.z, size.z, size.z/2)/100.f );
	m_pFrontWheel->setShape( pSceneNode, m_pWheelShape, restitution, friction, .1f, position+frontPosition, frontOrientation );
	m_pFrontWheel->getBulletRigidBody()->setActivationState(DISABLE_DEACTIVATION); // !!!

	// setup rear wheel
	pSceneNode = pSceneManager->getRootSceneNode()->createChildSceneNode( generateUniqueName() );
	pSceneNode->attachObject( pSceneManager->createEntity( Ogre::SceneManager::PT_CUBE ) );
	pSceneNode->setScale( Ogre::Vector3(size.z, size.z, size.z/2)/100.f );
	m_pRearWheel->setShape( pSceneNode, m_pWheelShape, restitution, friction, .1f, position+rearPosition, rearOrientation );
	m_pRearWheel->getBulletRigidBody()->setActivationState(DISABLE_DEACTIVATION); // !!!
	
	// setup front wheel constraint ( a hinge constraint )
	const btTransform frontA(btQuaternion(0,0,0,1), btVector3(0,0,0));
	const btTransform frontB( OgreBulletCollisions::OgreBtConverter::to( frontOrientation ), OgreBulletCollisions::OgreBtConverter::to( frontPosition ));
	m_pFrontWheelConstraint = new btHingeConstraint( 
		*(m_pFrontWheel->getBulletRigidBody()),
		*(m_pRigidBody->getBulletRigidBody()), 
		frontA, frontB, true );
	pWorld->getBulletDynamicsWorld()->addConstraint( m_pFrontWheelConstraint, true );

	// setup rear wheel constraint ( 6-dof constraint )
	const btTransform rearA(btQuaternion(0,0,0,1), btVector3(0,0,0));
	const btTransform rearB( OgreBulletCollisions::OgreBtConverter::to( rearOrientation ),OgreBulletCollisions::OgreBtConverter::to( rearPosition ));
	m_pRearWheelConstraint = new btGeneric6DofConstraint(
		*(m_pRearWheel->getBulletRigidBody()),
		*(m_pRigidBody->getBulletRigidBody()), 
		rearA, rearB, true );
	m_pRearWheelConstraint->setAngularLowerLimit(btVector3(0,0,1));
	m_pRearWheelConstraint->setAngularUpperLimit(btVector3(0,0,-1));
	pWorld->getBulletDynamicsWorld()->addConstraint( m_pRearWheelConstraint, true );

	
	return false;
}

/**
 * Callback function that reads its data from a string
 *
 * Its a function of the ModelSimulator_read_callback_t type. takes the address of a pointer containing the 
 * current position in the string.
 */
static uint64_t __cdecl stringreader( uint64_t _custom, uint64_t _buffer, uint64_t _buffer_size ) // num_bytes_read ( custom, buffer_start, buffer_size )
{
    assert( _custom && _buffer && _buffer_size );

    const char **pp = reinterpret_cast<const char **>(_custom);
    char *p = reinterpret_cast<char *>(_buffer);
    size_t p_length = static_cast<size_t>(_buffer_size);
    size_t pp_length = strlen(*pp);

    if( pp_length <= p_length ) {
        memcpy(p, *pp, pp_length);
        *pp += pp_length;
        return static_cast<uint64_t>(pp_length);
    }
    else {
        memcpy(p, *pp, p_length);
        *pp += p_length;
        return static_cast<uint64_t>(p_length);
    }
}

/**
 * Load a simulator
 *
 * @param pModuleName Name om dynamic library to load
 * @param resourceName Name of resource of the text file to sent to the simulator
 * @param groupName Name of resource group holding the resource.
 */
bool FmsVehicle::loadSimulator( const TCHAR *pModuleName, const Ogre::String &resourceName, const Ogre::String &groupName )
{
	Ogre::DataStreamPtr stream = Ogre::ResourceGroupManager::getSingletonPtr()->openResource( resourceName, groupName );
	Ogre::String parContent = stream->getAsString();
	stream->close();
	const char *p = parContent.c_str();
	if( m_simulator.load(pModuleName, stringreader, reinterpret_cast<uint64_t>(&p)) == false ){
		Ogre::LogManager::getSingletonPtr()->logMessage(Ogre::String("ERROR (FmsVehicle::loadSimulator) unable to load par: ") + resourceName);
		return false;
	}
	return true;
}

/**
 * Frees loaded resources
 *
 * First detaches visual and physical
 */
void FmsVehicle::unload()
{
	m_simulator.destroy();

	if( m_pWheelShape ) {
		delete m_pWheelShape;
		m_pWheelShape = NULL;
	}
	if( m_pFrontWheel ) {
		delete m_pFrontWheel;
		m_pFrontWheel = NULL;
	}
	if( m_pFrontWheelConstraint ) {
		delete m_pFrontWheelConstraint;
		m_pFrontWheelConstraint = NULL;
	}


	// todo: detach from phyics world
	if( m_pCollisionShape ) {
		delete m_pCollisionShape;
		m_pCollisionShape = NULL;
	}

	// destroy rigid body before deleting the ogre stuff becouse 
	// is contains reference to the visual object?
	if( m_pRigidBody ) {
		delete m_pRigidBody;
		m_pRigidBody = NULL;
	}

	// detach from visual world
	if( m_pSceneNode )
	{
		//todo: the following deletes all shicl nodes, but I dont think it
		// removes the node itself (this causes no memory leaks, but just takes
		// up a litte space untill the whole program unloads
		m_pSceneNode->removeAndDestroyAllChildren();
		m_pSceneNode=NULL;
	}

	/// todo: if i run the following, there will be an access violation when the ogre root 
	// is deleted, find out why, and "fix" it. (Again, this causes no memory leaks, but it
	// takes up unnessecary space untill the whole program exits)
	/*if( m_pEntity ) {
		delete m_pEntity;
		m_pEntity=NULL;
	}*/


}



/**
 * Attach an Input Device
 */
void FmsVehicle::attachInput( InputDevice *pInput )
{
	m_pInputDevice = pInput;
}

/**
 * Detach an Input Device
 */
void FmsVehicle::detachInput()
{
	m_pInputDevice = NULL;
}

/**
 * Capture data from the attached Input Device
 */
void FmsVehicle::captureInput()
{
	if( m_pInputDevice ) {
		m_pInputDevice->capture( m_environment.channel, 0, m_nChannels );
	}
}

Ogre::SceneNode *FmsVehicle::getSceneNode() const
{
	return m_pSceneNode;
}

/**
 * This function is called once before each frame is rendered, and before the physics simulator has stepped
 */
void FmsVehicle::onFrameStarted(const Ogre::FrameEvent &evt)
{
	// Capture the input here, this capture is done once for each frame
	captureInput();
	if( m_pRearWheelConstraint ) {
		const float angle = -m_environment.channel[3];
		m_pRearWheelConstraint->setAngularLowerLimit(btVector3(0,angle,1));
		m_pRearWheelConstraint->setAngularUpperLimit(btVector3(0,angle,-1));
	}

}

/** 
 * Convert captured input and to forces, and apply them
 *
 * Note: the inputs are captured in onFrameStarted, and not here
 * this way they only gets capturen once for each frame, 
 * since this function can be calles multiple times between frames.
 */
void FmsVehicle::onTick(const btDynamicsWorld *world, btScalar timeStep)
{

	// Apply forces given the cpatured input.
	// todo: check if sim is loaded:
	if(m_pRigidBody /*&& m_pModelSimulator*/) {
		btRigidBody *pBulletRigidBody;

		// No force on the front wheel (not even gravity) 
		pBulletRigidBody = m_pFrontWheel->getBulletRigidBody();
		assert( pBulletRigidBody != NULL && "Must not be null");
		pBulletRigidBody->clearForces();

		// No force on the rear wheel (not even gravity) 
		pBulletRigidBody = m_pRearWheel->getBulletRigidBody();
		assert( pBulletRigidBody != NULL && "Must not be null");
		pBulletRigidBody->clearForces();

		// Apply gravity on main body
		pBulletRigidBody = m_pRigidBody->getBulletRigidBody();
		assert( pBulletRigidBody != NULL && "Must not be null");
		pBulletRigidBody->clearForces();
		pBulletRigidBody->applyGravity();

		// get the orientation of the airplane
		btQuaternion orientation = pBulletRigidBody->getOrientation();
		btQuaternion orientationInv = orientation.inverse();
		// note: the above is just (-orientation), but there seems to be
		// an ambiguity with the negate operator.

		// Set up the apparent windspeed
		// note: the rest of m_environment is set up elsewhere
		btVector3 v;
		v = quatRotate(orientationInv, pBulletRigidBody->getLinearVelocity());
		m_environment.linearVelocityX = static_cast<float>(v.getX());
		m_environment.linearVelocityY = static_cast<float>(v.getY());
		m_environment.linearVelocityZ = static_cast<float>(v.getZ());
		v = quatRotate(orientationInv, pBulletRigidBody->getAngularVelocity());
		m_environment.angularVelocityX = static_cast<float>(v.getX());
		m_environment.angularVelocityY = static_cast<float>(v.getY());
		m_environment.angularVelocityZ = static_cast<float>(v.getZ());

		// call the model simulator to get the forces, given the environment
		// todo: move the setStepSize somwhere else
		ModelSimulator_forces_t forces = { sizeof(ModelSimulator_forces_t) };
		m_simulator.setStepSize(static_cast<float>(timeStep));
		m_simulator.step( m_environment, forces );
		
		// apply the forces to the object,
		// but first, convert the forces to world coodrinates
		btVector3 force(forces.force_x, forces.force_y, forces.force_z);
		btVector3 torque(forces.torque_x, forces.torque_y, forces.torque_z);
		pBulletRigidBody->applyForce(quatRotate(orientation, force), btVector3(0,0,0));
		pBulletRigidBody->applyTorque(quatRotate(orientation, torque));
	}
}









